#include "Utility/Algebra/AlgebraModule.h"
#ifndef SG_USE_MPI
#include "Assembly.h"

#include <iostream>

#include "Utility/Logging/SGLogger.h"
#include "Utility/Algebra/Factory.h"
#include "Utility/Algebra/LinearSolver.h"
#include "Utility/Algebra/sgEigenSpace.h"

#include "DataStructure/Input/Solver/DOFDirections.h"
#include "DataStructure/Input/Solver/FieldTypes.h"

#include "DBManager/DBAPI.h"
#include "DBManager/IService/Input/Constraint/ISPCService.h"
#include "DBManager/IService/Input/Load/ISPCDService.h"
#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Subcase/ISubcaseService.h"
#include "DBManager/IService/Input/Parameter/IParameterService.h"
#include "DBManager/IService/Input/Load/ILoadCombinationService.h"

using SG::Algebra::Matrix;
using SG::Algebra::Vector3D;
using SG::DataStructure::Common::Id;
using SG::DataStructure::Common::Real;
using SG::DataStructure::Common::Status;
using SG::DataStructure::FEM::DynaExcitType;
using SG::DataStructure::FEM::SOLType;
using SG::DataStructure::FEM::DOFDirections;
using SG::DataStructure::FEM::DofSet;
using SG::DataStructure::FEM::Field_Types;
using SG::DataStructure::FEM::NodeData;
using SG::DataStructure::FEM::Subcase;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::EigenSparse;
using SG::FEMSolver::EigenVector;
using SG::FEMSolver::TriArray;
using SG::FEMSolver::TriEle;
using SG::Algebra::convertTripletsToMKLSparse;

using namespace SG::DBManager;

/** @brief 调用eigen库实现矩阵分块
 *
 */
void AssemblyEigen::Matrixchunks (const EigenSparse& sparse,
                                  std::size_t        splitIndex,
                                  EigenSparse& _OUT  mat1,
                                  EigenSparse& _OUT  mat2,
                                  EigenSparse& _OUT  mat3,
                                  EigenSparse& _OUT  mat4)
{
    TriArray    triplet1;
    TriArray    triplet2;
    TriArray    triplet3;
    TriArray    triplet4;

    for (int k = 0; k < sparse.outerSize (); ++k)
    {
        for (Eigen::SparseMatrix<Real>::InnerIterator it (sparse, k); it; ++it)
        {
            const Eigen::Index& row = it.row ();
            const Eigen::Index& col = it.col ();
            const auto val = it.value ();
            if (row < splitIndex)
            {
                if (col < splitIndex)
                {
                    triplet1.emplace_back (Eigen::Triplet<Real> (row, col, val));
                }
                else  // tmp2 >= splitIndex
                {
                    triplet2.emplace_back (Eigen::Triplet<Real> (row, col - splitIndex, val));
                }
            }
            else
            {
                if (col < splitIndex)
                {
                    triplet3.emplace_back (Eigen::Triplet<Real> (row - splitIndex, col, val));
                }
                else  // tmp2 >= splitIndex
                {
                    triplet4.emplace_back (Eigen::Triplet<Real> (row - splitIndex, col - splitIndex, val));
                }
            }
        }
    }
    auto m = sparse.cols ();
    mat1   = Eigen::SparseMatrix<Real> (splitIndex, splitIndex);
    // mat1.setFromTriplets (triplet1.begin (), triplet1.end ());
    convertTripletsToMKLSparse (triplet1.begin (), triplet1.end (), mat1);

    mat2 = Eigen::SparseMatrix<Real> (splitIndex, m - splitIndex);
    // mat2.setFromTriplets (triplet2.begin (), triplet2.end ());
    convertTripletsToMKLSparse (triplet2.begin (), triplet2.end (), mat2);
    
    mat3 = Eigen::SparseMatrix<Real> (m - splitIndex, splitIndex);
    // mat3.setFromTriplets (triplet3.begin (), triplet3.end ());
    convertTripletsToMKLSparse (triplet3.begin (), triplet3.end (), mat3);
    
    mat4 = Eigen::SparseMatrix<Real> (m - splitIndex, m - splitIndex);
    // mat4.setFromTriplets (triplet4.begin (), triplet4.end ());
    convertTripletsToMKLSparse (triplet4.begin (), triplet4.end (), mat4);
}

/** @brief 计算mpc导致的约束方程， um = G *u_n */
void AssemblyEigen::ComputeMPCControlMatrix ()
{
    auto n     = GetAcctiveDOFSize ();
    auto nDofM = m_DOFHandler.GetSlaveDOFSize ();

    auto Rmn = EigenSparse (nDofM, n);
    // Rmn.setFromTriplets (m_RnTri.begin (), m_RnTri.end ());
    convertTripletsToMKLSparse (m_RnTri.begin (), m_RnTri.end (), Rmn);
    m_RnTri.clear ();

    auto Rmm = EigenSparse (nDofM, nDofM);
    // Rmm.setFromTriplets (m_RmTri.begin (), m_RmTri.end ());
    convertTripletsToMKLSparse (m_RmTri.begin (), m_RmTri.end (), Rmm);
    m_RmTri.clear ();

    // 求系数矩阵 Rmm 的逆

    // Eigen::SparseLU<EigenSparse> solver;
    // solver.compute (Rmm);
    // auto RInv = solver.solve (I);

    // setting sparse solver
    SGTrace << "inverse Rmm start";
    EigenSparse RInv;
    if (SG::Algebra::isDiagonal (Rmm, 1e-6))
    {
        SGInfo << "Rmm is Diagonal !!!! ";
        RInv = Rmm.diagonal ().asDiagonal ().inverse ();
    }
    else
    {
        SGInfo << "Rmm is not Diagonal !!!! ";
        EigenSparse I (nDofM, nDofM);
        I.setIdentity ();
        std::string solver_type = "TPardiso<Real_t>";
        auto        solver      = reinterpret_cast<SG::Algebra::LinearSolver*> (SG::Base::Factory::GetInstance ()->Produce (solver_type));
        solver->compute (Rmm);

        solver->solve (I, _OUT RInv);
        if (Eigen::Success != solver->info ())
        {
            std::cerr << "Error deal with Constraint Equation !!! " << std::endl;
            return;
        }
        delete solver;
    }
    SGTrace << "inverse Rmm fini";

    m_Gm = RInv * Rmn * -1.0;
}


Status AssemblyEigen::AdjustMPCLoad (const EigenVector& FmSet, EigenVector& _OUT FnSet) const
{
    // 若没有MPC直接返回
    const auto nSlaveDOF = static_cast<Eigen::Index>(m_DOFHandler.GetSlaveDOFSize());  
    if (nSlaveDOF == 0)
    {
        return Status::STATUS_Success;
    }
    // 检查维度
    if ( ( GetAcctiveDOFSize () != FnSet.size()) or ( m_DOFHandler.GetSlaveDOFSize () != FmSet.size()) )
    {   
        SGError << __FILE__ << ":" << __LINE__ << " Error: The dimensions are not match";
        return Status::STATUS_Failure;
    }

    // 处理荷载MPC约束
    FnSet += m_Gm.transpose() * FmSet;
    
    return Status::STATUS_Success;
}

Status AssemblyEigen::AdjustSPCLoad (EigenVector& FnSet) const 
{
    // 检查维度
    if (FnSet.size() != GetAcctiveDOFSize())
    {
        SGError << __FILE__ << ":" << __LINE__ << " Error: The dimensions are not match";
        return Status::STATUS_Failure;
    }

    // 修正荷载，减去 SPC 强制位移带来的附加荷载 即"b = b.head(nKDim) - m_kfs * m_Ys"
    const auto nKDim = static_cast<Eigen::Index> (m_DOFHandler.ComputeSparseMatrixSize());  // 自由无约束自由度维度
    FnSet = FnSet.head(nKDim) - m_kfs * m_Ys;
    
    return Status::STATUS_Success;
}

AssemblyEigen& AssemblyEigen::GetInstance ()
{
    static AssemblyEigen Instance;
    return Instance;
}

/** @brief 初始化, 使用网格中的所有节点Id, 生成id到节点在数据库索引的映射关系
 *
 * 本函数对节点id进行了升序排列，因此后续查找节点索引可以采用二分查找。</p>
 * 如果要传入新的网格进行计算， 需要调用本类的 `Clear()`接口
 */
void AssemblyEigen::Initialize (const SG::DBManager::DBServiceFactorySP        dbServiceFactory,
                                const SG::DataStructure::FEM::Subcase&         subcase,
                                const std::set<SG::DataStructure::Common::Id>& loadCases)
{
    // 避免重复初始化
    if (m_isInitialized)
    {
        return;
    }

    // 清空临时变量
    Clear ();
    /** 将 m_isInitialized 显式修改为true。
     * 只有调用 Clear()方法后，再次调用本方法Initialize (), 才会调用后边的语句
     */
    m_isInitialized = true;


    // 计算对用工况的自由度排序
    m_fieldType = Field_Types::FIELD_Elastic;
    if (SOLType::SOL_NLSCSH == subcase.GetSOLType ())
    {
        m_fieldType = Field_Types::FIELD_Thermal;
    }

    const auto nodeIds = DBAPI::GetAllNodeId (dbServiceFactory.get());
    m_DOFHandler = SG::FEMSolver::DOFHandler (nodeIds, m_fieldType);

    m_DOFHandler.Compute (dbServiceFactory, subcase);

    // 方程自由度，包括独立自由度和spc约束自由度
    const auto nSparseMatrix = this->GetAcctiveDOFSize ();
    // std::cout << "$$$$$$ nSparseMatrix " << nSparseMatrix << std::endl;
    auto Ff = EigenVector (nSparseMatrix);
    Ff.setZero (nSparseMatrix);

    const auto nGlobalMSets = m_DOFHandler.GetSlaveDOFSize ();
    auto       Fm           = EigenVector (nGlobalMSets);
    Fm.setZero (nGlobalMSets);

    std::size_t sIndex = 0;
    for (auto loadcase : loadCases)
    {
        m_loadmap[loadcase] = sIndex;
        m_FnSet.push_back (Ff);  // Todo 确认是否是深copy
        m_FmSet.push_back (Fm);
        ++sIndex;
    }

    m_Gm = EigenSparse ();
}

Status AssemblyEigen::FindNodeIndex (SG::DataStructure::Common::Id nodeId, _OUT std::size_t& nodeIndex) const
{
    nodeIndex = m_DOFHandler.FindNodeIndex (nodeId);

    if (-1UL == nodeIndex)
    {
        return Status::STATUS_Failure;
    }

    return Status::STATUS_Success;
}

void AssemblyEigen::FindNodeIndex (const SG::DataStructure::Common::Id* nodeIds, _OUT std::vector<std::size_t>& nodeIndexes) const
{
    for (auto& index : nodeIndexes)
    {
        index = m_DOFHandler.FindNodeIndex (*nodeIds);
        ++nodeIds;  // 移动指针，指向下一个要处理的节点
    }
}

void AssemblyEigen::GetGlobalDOFId (const std::vector<std::size_t>&       nodeIndexes,
                                    const SG::DataStructure::FEM::DofSet& DOFs,
                                    _OUT std::vector<int>& dofGlobalIds) const
{
    m_DOFHandler.GetGlobalIndex (nodeIndexes, DOFs, dofGlobalIds);
}

void AssemblyEigen::GetGlobalDOFId (const std::size_t                            nodeIndex,
                                    const SG::DataStructure::FEM::DOFDirections& dof,
                                    _OUT int&                                    dofGlobalId) const
{
    dofGlobalId = m_DOFHandler.GetGlobalIndex (nodeIndex, dof);
}

/** @brief 查询节点指定自由度的全局编号
 *
 * 首先通过节点id，找到节点的索引， 再读取该节点对应的自由度全局编号
 */
void AssemblyEigen::ComputeGlobalDOFId (const SG::DataStructure::Common::Id   nodeId,
                                        const SG::DataStructure::FEM::DofSet& DOFs,
                                        _OUT std::vector<int>& dofGlobalIds)
{
    auto nodeIndex = m_DOFHandler.FindNodeIndex (nodeId);
    if (-1 == nodeIndex)
    {
        // 找不到编号为nodeId的节点，将全局自由度全部设为非激活自由度
        for (auto& id : dofGlobalIds)
        {
            id = -1;
        }
        return;
    }

    m_DOFHandler.GetGlobalIndex ({ static_cast<std::size_t> (nodeIndex) }, DOFs, dofGlobalIds);
}


void AssemblyEigen::ProcessLoadConstraint (EigenVector& _OUT TotalLoad) const
{
    const std::size_t nActiveDOF = GetAcctiveDOFSize();
    const std::size_t nSlaveDOF = m_DOFHandler.GetSlaveDOFSize ();

    EigenVector Fn = TotalLoad.head(static_cast<Eigen::Index>(nActiveDOF)); // FnSet  
    EigenVector Fm = TotalLoad.tail(static_cast<Eigen::Index>(nSlaveDOF));  // FmSet

    // MPC 处理
    AdjustMPCLoad(Fm, Fn);

    // SPC 处理
    AdjustSPCLoad(Fn);


    // 调整输出维度（SPC 处理后 Fn 已为自由端载荷，截断 TotalLoad 至自由端维度）
    TotalLoad.head(Fn.size()) = Fn;
    TotalLoad.conservativeResize(static_cast<Eigen::Index>(GetFreeDOFSize()));
}


/** @brief 矩阵组装通用过程， 整体刚度矩阵，应力刚度，（质量矩阵）等按照是否做为mpc从自由度组装到不同的矩阵块
 *
 */
auto Assemby = [] (int                     nSparse,
                   int                     nEffectiveDof,
                   const std::vector<int>& globalIndex,
                   const Matrix&           stiffness,
                   TriArray&               Kff,
                   TriArray&               Kfm,
                   TriArray&               Kmm) -> void {
    std::size_t iIndex{ 0 }, jIndex{ 0 };
    for (const auto& iDof : globalIndex)
    {
        // iDof < 0, 该自由度没有激活
        assert (iDof >= 0 && iDof < nEffectiveDof);
        if (iDof < nSparse)
        {
            jIndex = 0;
            for (const auto& jDof : globalIndex)
            {
                assert (jDof > -1 && jDof < nEffectiveDof);
                assert (!std::isnan (stiffness (iIndex, jIndex)));
                if (jDof < nSparse)
                {
                    Kff.push_back (TriEle (iDof, jDof, stiffness (iIndex, jIndex)));
                }
                else
                {
                    Kfm.push_back (TriEle (iDof, jDof - nSparse, stiffness (iIndex, jIndex)));
                }
                ++jIndex;
            }
        }
        else
        {
            jIndex = 0;
            for (const auto& jDof : globalIndex)
            {
                assert (jDof > -1 && jDof < nEffectiveDof);
                assert (!std::isnan (stiffness (iIndex, jIndex)));
                // jIndex < nSparse, Kmf不需要组装
                if (jDof >= nSparse)
                {
                    Kmm.push_back (TriEle (iDof - nSparse, jDof - nSparse, stiffness (iIndex, jIndex)));
                }
                ++jIndex;
            }
        }
        ++iIndex;
    }
};

void AssemblyEigen::AccumulateStiffness (const std::vector<int>& globalIndex, const Matrix& stiffness)
{
    const int nSparse       = static_cast<int> (GetAcctiveDOFSize ());
    const int nEffectiveDof = static_cast<int> (GetAcctiveDOFSize () + m_DOFHandler.GetSlaveDOFSize ());

    Assemby (nSparse, nEffectiveDof, globalIndex, stiffness, m_TriKff, m_TriKfm, m_TriKmm);
}

void AssemblyEigen::AccumulateCoupledMass (const std::vector<int>& globalIndex, const Matrix& mass)
{
    const int nSparse       = static_cast<int> (GetAcctiveDOFSize ());
    const int nEffectiveDof = static_cast<int> (GetAcctiveDOFSize () + m_DOFHandler.GetSlaveDOFSize ());

    Assemby (nSparse, nEffectiveDof, globalIndex, mass, m_TriCoupledMff, m_TriCoupledMfm, m_TriCoupledMmm);
}

/** @brief 累加应力刚度的贡献 */
void AssemblyEigen::AccumulateStressStiffness (const std::vector<int>& globalIndex, const Matrix& kSigma)
{
    const int nSparse       = static_cast<int> (GetAcctiveDOFSize ());
    const int nEffectiveDof = static_cast<int> (GetAcctiveDOFSize () + m_DOFHandler.GetSlaveDOFSize ());
    Assemby (nSparse, nEffectiveDof, globalIndex, kSigma, m_TriK4ff, m_TriK4fm, m_TriK4mm);
}

SG::DataStructure::Common::Status AssemblyEigen::AccumulateLoad (SG::DataStructure::Common::Id sid, const std::vector<int>& globalDof, const Matrix& load)
{
    assert (load.GetRowSize () == globalDof.size ());
    assert (1 == load.GetColSize ());
    assert (0 != m_loadmap.count (sid));

    Status status{Status::STATUS_Success};

    auto& Ff = m_FnSet[m_loadmap[sid]];  // 将变量通过引用绑定
    auto& Fm = m_FmSet[m_loadmap[sid]];

    const int nSparse       = static_cast<int> (GetAcctiveDOFSize ());
    const int nEffectiveDof = static_cast<int> (GetAcctiveDOFSize () + m_DOFHandler.GetSlaveDOFSize ());

    auto nComponent = load.GetRowSize ();
    for (std::size_t index{ 0 }; index < nComponent; ++index)
    {
        const auto globalDofId = globalDof[index];
        const auto fi          = load (index, 0);
        // "载荷不能加到非激活自由度 && 全局自由度编号不能发生越界
        if (globalDofId < 0 || globalDofId >= nEffectiveDof)
        {
            if (fabs (fi) > 1.0e-12)
            {
                SGWarn << __FILE__ << ":" << __LINE__ << "invaid load component";
                status = Status::STATUS_Failure;
            }
            continue;
        }

        // 按照自由度是否为 mpc从自由度分别组装
        if (globalDofId < nSparse)
        {
            Ff (globalDofId) += fi;
        }
        else
        {
            Fm (globalDofId - nSparse) += fi;
        }
    }

    return status;
}

void AssemblyEigen::AccumulateLoad (SG::DataStructure::Common::Id sid, const SG::FEMSolver::EigenVector& globalLoad)
{
    assert (globalLoad.size () == static_cast<Eigen::Index>(GetTotalDOFSize()));
    assert (0 != m_loadmap.count (sid));
    
    m_FnSet[m_loadmap[sid]] = globalLoad.head (GetAcctiveDOFSize ());
    m_FmSet[m_loadmap[sid]] = globalLoad.tail(m_DOFHandler.GetSlaveDOFSize());
}

void AssemblyEigen::AccumulateControlMatrix (int gdofn, int gdofm, SG::DataStructure::Common::Real val)
{
    const auto nActiveDofs  = static_cast<int> (GetAcctiveDOFSize ());             // 网格中自由的自由度数量
    const auto nGlobalMSets = static_cast<int> (m_DOFHandler.GetSlaveDOFSize ());  // 网格中多点约束方程的数量

    // 对应元素组装到约束方程的行号, 即组装到哪一个约束方程中
    auto iEquation = gdofm - nActiveDofs;
    assert (iEquation >= 0);

    if (gdofn < 0)  // 主自由度为非激活自由度
    {
        SGError << " 主自由度必须为为自由状态" << std::endl;
        return;
    }
    assert (gdofn < (nActiveDofs + nGlobalMSets));
    assert (!std::isnan (val));

    // 避免将 0.0填入控制方程，导致影响控制方程的稀疏性
    // 测试发现此次过滤0.0的实现存在问题
    // if (fabs (val) > 1.0e-18)
    // {
    if (gdofn < nActiveDofs)
    {
        m_RnTri.push_back (TriEle (iEquation, gdofn, val));
    }
    else
    {
        m_RmTri.push_back (TriEle (iEquation, gdofn - nActiveDofs, val));
    }
    // }
}

/** @brief 累加约束方程
 * @param [in] gdofn 方程系数列 对应的全局自由度
 * @param [in] gdofm 方程系数行 对应的全局自由度， 约束方程对哪一个约束方程又贡献
 *
 */
void AssemblyEigen::AccumulateControlMatrix (const std::vector<int>& gdofn, const std::vector<int>& gdofm, const Matrix& RnRm)
{
    assert (gdofn.size () == RnRm.GetColSize ());
    assert (gdofm.size () == RnRm.GetRowSize ());

    const auto nDofM        = gdofm.size ();
    for (std::size_t row = 0; row < nDofM; ++row)
    {
        const auto gm = gdofm.at (row);
        // 依次处理矩阵的列
        std::size_t col{ 0 };
        for (auto dof : gdofn)
        {
            const auto& val = RnRm (row, col);
            AccumulateControlMatrix (dof, gm, val);
            ++col;
        }
    }
}

void AssemblyEigen::ClearBCLoad ()
{
    m_BCLoadIdSet.clear();

    m_FnBCSet.clear ();
    m_FmBCSet.clear ();
}

void AssemblyEigen::AccumulateBCLoad (SG::DataStructure::Common::Id sid, const std::vector<int>& globalDof, const SG::Algebra::Matrix& load)
{
    assert (load.GetRowSize () == globalDof.size ());
    assert (1 == load.GetColSize ());
    if (0 == m_BCLoadIdSet.count (sid))
    {
        // 激活当前工况
        m_BCLoadIdSet.insert (sid);
        // 方程自由度，包括独立自由度和spc约束自由度    
        const auto nSparseMatrix = this->GetAcctiveDOFSize ();
        // std::cout << "$$$$$$ nSparseMatrix " << nSparseMatrix << std::endl;
        auto Ff = EigenVector (nSparseMatrix);
        Ff.setZero (nSparseMatrix);
        m_FnBCSet[sid] = Ff;
        const auto nGlobalMSets = m_DOFHandler.GetSlaveDOFSize ();
        auto       Fm           = EigenVector (nGlobalMSets);
        Fm.setZero (nGlobalMSets);
        m_FmBCSet[sid] = Ff;

    }

    auto& Ff = m_FnBCSet[sid];  // 将变量通过引用绑定
    auto& Fm = m_FmBCSet[sid];

    const int nSparse       = static_cast<int> (GetAcctiveDOFSize ());
    const int nEffectiveDof = static_cast<int> (GetAcctiveDOFSize () + m_DOFHandler.GetSlaveDOFSize ());

    auto nComponent = load.GetRowSize ();
    for (std::size_t index{ 0 }; index < nComponent; ++index)
    {
        const auto globalDofId = globalDof[index];
        const auto fi          = load (index, 0);
        // "载荷不能加到非激活自由度 && 全局自由度编号不能发生越界
        if (globalDofId < 0 || globalDofId >= nEffectiveDof)
        {
            if (fabs (fi) > 1.0e-12)
            {
                SGWarn << __FILE__ << ":" << __LINE__ << "invaid load component";
            }
            continue;
        }

        // 按照自由度是否为 mpc从自由度分别组装
        if (globalDofId < nSparse)
        {
            Ff (globalDofId) += fi;
        }
        else
        {
            Fm (globalDofId - nSparse) += fi;
        }
    }
}

/** @brief    autospc 方向查找，并使用乘大数法约束改方向
 * 
 * @param    dbServiceFactory 
 * @param    dofGlobalIndexes 
 * @param    kDim 
 * @param    nodeId      仅仅是为了 输出节点编号
 * @param    maxValue 
 * @param    DirectionStr
 * @param    K 
 * @author   wanghuan
 */
void applyAutoSPC (const DBServiceFactorySP         dbServiceFactory,
                   const std::vector<int>&          dofGlobalIndexes,
                   const int                        kDim,
                   const Id                         nodeId,
                   Real                             maxValue,
                   const std::vector<std::string>&  DirectionStr,
                   SG::FEMSolver::EigenSparse& _OUT K)
{
    // find dof which belong to f-set 
    std::vector<int> activeDofSet (0);
    std::vector<int> directionSet (0);
    int              direction = 0; // mark dof type "UX UY UZ RotX ... "
    for (const auto& gDof : dofGlobalIndexes)
    {
        if (gDof > -1 && gDof < kDim)
        {
            activeDofSet.push_back (gDof);
            directionSet.push_back (direction);
        }
        ++direction;
    }

    // don't have free dof
    const auto nActiveDofs = activeDofSet.size ();
    if (0 == nActiveDofs)
    {
        return;
    }

    // 查找是否存在需要添加autoSPC的方向
    std::vector<int> autoSPCGloDofIds;
    std::vector<int> spcDirections;
    
    if (1 == nActiveDofs)
    {
        if (K.coeffRef (activeDofSet[0], activeDofSet[0]) < 1.0e-8)
        {
            autoSPCGloDofIds = activeDofSet;
            spcDirections = directionSet;
        }
    }
    else
    {
        // 读取矩阵块
        Eigen::Matrix3d M;
        M (0, 0) = K.coeffRef (activeDofSet[0], activeDofSet[0]);
        M (0, 1) = K.coeffRef (activeDofSet[0], activeDofSet[1]);
        M (1, 1) = K.coeffRef (activeDofSet[1], activeDofSet[1]);
        if (2 == nActiveDofs)
        {
            M (0, 2) = 0.0;
            M (1, 2) = 0.0;
            M (2, 2) = 0.0;
        }
        else
        {
            M (0, 2) = K.coeffRef (activeDofSet[0], activeDofSet[2]);
            M (1, 2) = K.coeffRef (activeDofSet[1], activeDofSet[2]);
            M (2, 2) = K.coeffRef (activeDofSet[2], activeDofSet[2]);
        }
        // sym
        M (1, 0) = M (0, 1);
        M (2, 1) = M (1, 2);
        M (2, 0) = M (0, 2);

        // 特征值计算  The eigenvalues are sorted in increasing order
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver (M);
        if (Eigen::Success != eigensolver.info ())
        {
            abort ();
        }
        Eigen::Vector3d LAMBDA = eigensolver.eigenvalues ().real ();

        auto KMax = 0.0;
        for (int i{0}; i < 3; ++i)
        {
            const auto ki = fabs(LAMBDA (i));
            if (ki > KMax)
            {
                KMax = ki;
            }
        }

        // 最大特征值为0.0， 所有约束方向都处理
        if (KMax < 1.0e-8)
        {
            // 此时矩阵块全是0，此处没有自由度激活
            // SGDebug << "几乎不会发生";
            autoSPCGloDofIds = activeDofSet;
            spcDirections = directionSet;
        }
        else
        {
            // 寻找spc方向
            for (std::size_t i{0}; i < nActiveDofs; ++i)
            {
                if (M(i, i) < KMax*1.0e-8)
                {
                    autoSPCGloDofIds.push_back(activeDofSet[i]);
                    spcDirections.push_back(directionSet[i]);
                }
            }
        }
    }

    auto nConstraintDOF = autoSPCGloDofIds.size ();
    for (std::size_t index = 0; index < nConstraintDOF; ++index)
    {
        auto posi               = autoSPCGloDofIds[index];
        K.coeffRef (posi, posi) = 1.0e10 * maxValue;
        auto& DircStr = DirectionStr[spcDirections[index]];
        SGDebug << "auto spc, node id " << nodeId << " ;Direction " << DircStr;
    }
}

/** @brief 读取并组装 spc对应的位移矩阵
 *
 * 对于GRID的“PS”, 由于位移值为0.0. 因此不需要处理有关的值
 * @param      dbServiceFactory 数据库
 * @param      spcId     单点约束集合编号
 * @param      nTotalSet 模型中所有激活自由度数量
 * @param      nfSet     独立自由度数量
 * @param[out] Ys        spc对应的位移矩阵, 向量应该提取指定相应的size, 并且将所有元素初始化为0.0;
 */
void assemblyYs (const SG::DBManager::DBServiceFactorySP dbServiceFactory,
                 const SG::FEMSolver::DOFHandler&        DOFHandler,
                 const Subcase&                          subcase,
                 int                                     nTotalSet,
                 int                                     nfSet,
                 _OUT EigenVector&                       Ys)
{
    const auto nSDOFSet = nTotalSet - nfSet;
    Ys                  = EigenVector (nSDOFSet);
    Ys.setZero (nSDOFSet);

    /** spc 和spc1 数据 */
    SG::DataStructure::FEM::SPCDataArray spcSet;
    dbServiceFactory->get<ISPCService> ()->FindBySId (subcase.GetSPCId(), _OUT spcSet);
    /** 叠加spcd数据 */
    SG::DataStructure::FEM::SPCDataArray spcdSet;
    dbServiceFactory->get<ISPCDService> ()->FindBySId (subcase.GetLoadId(), spcdSet);
    spcSet.insert (spcSet.end (), spcdSet.begin (), spcdSet.end ());

    for (auto& spc : spcSet)
    {
        auto nodeIndex = DOFHandler.FindNodeIndex (spc.m_g);
        if (-1 == nodeIndex)
        {
            std::cout << "warning, shouldn't constraint this node [ " << spc.m_g << " ]" << std::endl;
            continue;
        }

        DofSet     indexes = spc.GetIndexes ();
        const auto dofSize = indexes.size ();
        for (std::size_t index = 0; index < dofSize; ++index)
        {
            const auto posi = DOFHandler.GetGlobalIndex (nodeIndex, indexes[index]);
            // 跳过非激活自由度
            if (posi < 0)
            {
                std::cout << "shouldnot constraint " << spc.m_g << " direction " << indexes[index] << std::endl;
                continue;
            }

            // 仅仅需要处理非零情况
            if (fabs (spc.m_d) > 1.0e-10)
            {
                Ys (posi - nfSet) = spc.m_d;
            }
        }
    }
}

/** @brief    静凝聚计算，并且释放非对角及要消去的块占用内存
 * 
 * 
 * @author   wanghuan
 * @date     2025.06.19
 */
auto condense = [](const EigenSparse& Gm, const EigenSparse& GmT, TriArray& TriKfm, TriArray& TriKmm, std::size_t n, std::size_t nDofM, EigenSparse& K) -> void
{
    // assembly non-diag block
    auto Knm = EigenSparse (n, nDofM);
    convertTripletsToMKLSparse (TriKfm.begin (), TriKfm.end (), Knm);
    TriKfm.clear();

    auto Kmm = EigenSparse (nDofM, nDofM);
    convertTripletsToMKLSparse (TriKmm.begin (), TriKmm.end (), Kmm);
    TriKmm.clear();

    K = K + GmT * Knm.transpose () + Knm * Gm + GmT * Kmm * Gm;
};

void AssemblyEigen::AssembleTotalCoupledMassMatrix()
{
    auto n     = GetAcctiveDOFSize ();              // 自由自由度数量
    auto nDofM = m_DOFHandler.GetSlaveDOFSize();    // 从自由度数量
    auto nTotalDof = n + nDofM;                     // 总自由度数量
    m_CoupledMTotal = EigenSparse(nTotalDof, nTotalDof);  // 初始化总质量矩阵

    // 构建总三元组容器，预分配内存
    TriArray mTotalTri;
    mTotalTri.reserve(
        m_TriCoupledMff.size() +    // 左上角块：m_CoupledM
        m_TriCoupledMfm.size() +    // 右上角块：Mfm
        m_TriCoupledMfm.size() +    // 左下角块：Mfm^T
        m_TriCoupledMmm.size()      // 右下角块：Mmm
    );

    // 左上角块：m_CoupledM（n x n），索引无偏移
    for (const auto& tri : m_TriCoupledMff)
    {
        mTotalTri.emplace_back(tri.row(), tri.col(), tri.value());
    }

    // 右上角块：Mfm（n x nDofM），列索引偏移n
    for (const auto& tri : m_TriCoupledMfm)
    {
        mTotalTri.emplace_back(tri.row(), tri.col() + n, tri.value());
    }

    // 左下角块：Mfm^T（nDofM x n），行索引偏移n
    for (const auto& tri : m_TriCoupledMfm)
    {
        mTotalTri.emplace_back(tri.col() + n, tri.row(), tri.value());
    }

    // 右下角块：Mmm（nDofM x nDofM），行和列索引均偏移n
    for (const auto& tri : m_TriCoupledMmm)
    {
        mTotalTri.emplace_back(tri.row() + n, tri.col() + n, tri.value());
    }

    // 转换为稀疏矩阵并压缩
    convertTripletsToMKLSparse(mTotalTri.begin(), mTotalTri.end(), m_CoupledMTotal);
    m_CoupledMTotal.makeCompressed();
}

void AssemblyEigen::Assembly (const DBServiceFactorySP dbServiceFactory)
{
    SGDebug << "== assembly start ";
    
    auto       n     = GetAcctiveDOFSize ();
    
    // 组装刚度矩阵，并且释放三元组
    SGTrace << "TriArray assemble SPMatrix start";
    m_K = EigenSparse (n, n);
    convertTripletsToMKLSparse (m_TriKff.begin (), m_TriKff.end (), m_K);
    m_TriKff.clear ();
    SGTrace << "TriArray assemble SPMatrix fini";
    
    if (m_needMass)
    {
        m_CoupledM = EigenSparse (n, n);
        convertTripletsToMKLSparse (m_TriCoupledMff.begin (), m_TriCoupledMff.end (), m_CoupledM);

        // 计算结构总自由度质量矩阵
        AssembleTotalCoupledMassMatrix(); 
        
        // 释放临时三元组内存，另外两个不释放是因为后续“质量矩阵多点约束处理”会用，但是静力求解重力荷载无需质量矩阵约束处理，后续需要再调整这方面架构
        m_TriCoupledMff.clear();
        //m_TriCoupledMfm.clear();
        //m_TriCoupledMmm.clear();

    }  
}

/** @brief 约束处理， 消去mpc的非独立自由度， 处理spc约束，添加autospc计算功能
 *
 * 使用线性消去处理mpc方程， 使用乘大数法处理spc计算功能， 自动进行autoSPC 处理
 */
void AssemblyEigen::Constraint (const SG::DBManager::DBServiceFactorySP dbServiceFactory, const SG::DataStructure::Common::Id subcaseId)
{
    auto       n     = GetAcctiveDOFSize ();
    auto       nDofM = m_DOFHandler.GetSlaveDOFSize ();
    const auto nKDim = static_cast<int> (m_DOFHandler.ComputeSparseMatrixSize ());

    // 处理多点约束方程
    if (NeedComputeControlMatrix ())
    {
        SGDebug << "== deal with Milti Point Constraint ";
        // 计算 m_Gm
        ComputeMPCControlMatrix ();
        auto GmT = m_Gm.transpose ();
    
        condense (m_Gm, GmT, m_TriKfm, m_TriKmm, n, nDofM, _OUT m_K);
    
        // 质量矩阵多点约束处理
        if (m_needMass)
        {
            condense (m_Gm, GmT, m_TriCoupledMfm, m_TriCoupledMmm, n, nDofM, _OUT m_CoupledM);
        }
    
        // 载荷右端项多点约束处理
        const auto nLoadCase = m_FnSet.size ();
        for (std::size_t iLoad{ 0 }; iLoad < nLoadCase; ++iLoad)
        {
            AdjustMPCLoad(m_FmSet[iLoad], m_FnSet[iLoad]);
        }
    
        // 右端项特殊边界考虑多点约束
        // const auto nBC = m_BCLoadIdSet.size();
        for (auto sid : m_BCLoadIdSet)
        {
            m_FnBCSet[sid] += m_Gm.transpose () * m_FmBCSet[sid];
        }
    }
    
    /**  处理单点约束（包括非零强制位移）
     */
    SGDebug << "== deal with Single Point Constraint ";
    auto para = dbServiceFactory->get<ISubcaseService> ()->FindSubCase (subcaseId);
    Subcase subcase(para);
    assemblyYs (dbServiceFactory, m_DOFHandler, subcase, n, nKDim, _OUT m_Ys);
    
    /** 将稀疏矩阵按照（1）被spc约束或者（2）做为独立自由度这两种情况 进行分块 */
    EigenSparse ksf;  // dummy
    EigenSparse kss;  // dummy
    AssemblyEigen::Matrixchunks (m_K, nKDim, _OUT m_K, _OUT m_kfs, _OUT ksf, _OUT kss);
    
    if (m_needMass)
    {
        EigenSparse msf;  // dummy
        EigenSparse mss;  // dummy
        // EigenSparse mfs;
        AssemblyEigen::Matrixchunks (m_CoupledM, nKDim, _OUT m_CoupledM, _OUT m_Mfs, _OUT msf, _OUT mss);
    }
    
    // auto SPC
    bool autospc = true;
    const auto PARAM = dbServiceFactory->get<IParameterService> ()->GetSolverParameter ();
    if (PARAM.Has ("AUTOSPC"))
    {
        SGError << PARAM["AUTOSPC"].PrettyPrintJsonString();
        if("NO"== PARAM["AUTOSPC"].GetString ())
        {
            autospc = false;
        }
    }
    
    if (autospc && (Field_Types::FIELD_Elastic == m_fieldType))
    {
        /** 找到对角元中的绝对值最大值
         *  寻找最大元素并不是autospc必须的，而是因为autospc 使用乘大数法而需要最大的对角线元素（绝对值）
         */ 
        Real maxValue{ 0.0 };
        for (int i = 0; i < nKDim; ++i)
        {
            auto tmp = fabs (m_K.coeffRef (i, i));
            if (tmp > maxValue)
            {
                maxValue = tmp;
            }
        }
    
        static DofSet    transDOFSet{ DOFDirections::DOF_Ux, DOFDirections::DOF_Uy, DOFDirections::DOF_Uz };
        static DofSet    rotDOFSet{ DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz };
    
        std::vector<int> dofGlobalIndexes (3);
        const auto&      nodeIds = m_DOFHandler.GetNodeIds ();
    
        auto             nNode = m_DOFHandler.GetNodeSize ();
        for (std::size_t iNode = 0; iNode < nNode; ++iNode)
        {
            auto nodeId = nodeIds[iNode];
            m_DOFHandler.GetGlobalIndex ({ iNode }, transDOFSet, _OUT dofGlobalIndexes);
            applyAutoSPC (dbServiceFactory, dofGlobalIndexes, nKDim, nodeId, maxValue, SG::DataStructure::FEM::TRAN_DIRECTION_STR, _OUT m_K);
            // rot
            m_DOFHandler.GetGlobalIndex ({ iNode }, rotDOFSet, _OUT dofGlobalIndexes);
            applyAutoSPC (dbServiceFactory, dofGlobalIndexes, nKDim, nodeId, maxValue, SG::DataStructure::FEM::ROT_DIRECTION_STR, _OUT m_K);
        }
    }
    
    // 压缩矩阵， 去掉 （_， _）占位元素
    m_K.makeCompressed();
    SGDebug << "== assembly fini " << std::endl;
}

void AssemblyEigen::ComputeBVector (SG::DataStructure::Common::Id loadCase, _OUT EigenVector& b) const
{
    auto it = m_loadmap.find (loadCase);
    if (it == m_loadmap.end ())
    {
        const auto nSparseMatrix = GetAcctiveDOFSize ();
        b.setZero(nSparseMatrix);
    }
    else
    {
        b = m_FnSet[it->second];
    }
    
    for (auto sid : m_BCLoadIdSet)
    {
        
        b += m_FnBCSet.at(sid);
    }
    
    AdjustSPCLoad(b);
}

void AssemblyEigen::ComputeBVector (const SG::DBManager::DBServiceFactorySP& dbServiceFactory, 
                                    const SG::DataStructure::FEM::LoadCombination& load, 
                                    _OUT EigenVector& b) const

{
    const auto nSparseMatrix = GetAcctiveDOFSize ();
    b.setZero (nSparseMatrix);

    // 定义递归lambda函数，用于处理当Load中嵌套Load的情况
    // 用std::function声明lambda类型,因为直接用lambda匿名函数auto无法递归
    std::function<void(Id, Real)> addLoad;
    addLoad = [this, &dbServiceFactory, &b, &addLoad](Id loadId, Real totalFactor) -> void 
    {
        // 尝试查询当前ID是否为载荷组合
        SG::DataStructure::FEM::LoadCombination subLoad;
        const auto isLoadCase = dbServiceFactory -> get<ILoadCombinationService>() -> FindById (loadId, subLoad);

        if (Status::STATUS_Success == isLoadCase) 
        {
            // 若是嵌套载荷组合：递归处理其子项（累积系数）
            Real newFactor = totalFactor * subLoad.m_facotor;
            for (const auto& subItem : subLoad.m_Items) 
            {
                addLoad(subItem.m_subSet, newFactor * subItem.m_subfactor);
            }
        } 
        else 
        {
            // 为普通载荷集时，为该递归 lambda函数的递归基
            auto it = m_loadmap.find(loadId);
            if (it != m_loadmap.end()) 
            {
                b += totalFactor * m_FnSet[it->second];
            } else 
            {
                SGFatal << "The Load Set's ID cant be find";
            }
        }
    };

    // 调用递归函数，处理顶层载荷组合
    for (const auto& topItem : load.m_Items) 
    {
        addLoad(topItem.m_subSet, load.m_facotor * topItem.m_subfactor);
    }

    //叠加BCLoad 
    for (auto sid : m_BCLoadIdSet)
    {
        b += m_FnBCSet.at(sid);
    }

    // 调整SPCLoad
    AdjustSPCLoad(b);
}

void AssemblyEigen::ComputeFreqSPCDLoad (SG::DataStructure::Common::Id         sid,
                                         SG::DataStructure::FEM::DynaExcitType type,
                                         SG::DataStructure::Common::Real       w,
                                         SG::Algebra::ComplexVec_t& _OUT       b) const
{
    if ((DynaExcitType::DYNAEXC_Force == type) || (DynaExcitType::DYNAEXC_Unkown  == type))
    {
        SGFatal << "force load can't using this interface";
    }
    // disp load
    auto Us = m_Ys;

    if (DynaExcitType::DYNAEXC_Velc == type)
    {
        SGFatal << "enforce vec need complex type";
    }

    // acce load
    if (DynaExcitType::DYNAEXC_Acce == type)
    {
        Us *= (-1.0/ w*w);
    }

    // Error 目前无法获得reli阻尼
    // m_FnSet 在之前的流程中已经进行了多点约束的处理
    const auto nFSet = m_DOFHandler.ComputeSparseMatrixSize ();
    auto it       = m_loadmap.find (sid);
    if (it == m_loadmap.end ())
    {
        assert (0);
        return;
    }
    const auto index = it->second;
    b = m_FnSet[index].head(nFSet) - (m_Mfs * (-1.0* w* w) + m_kfs) * Us;
}

const EigenSparse AssemblyEigen::AssembleStressStiffnessMatrix ()
{
    auto nNSet = GetAcctiveDOFSize ();
    auto nMSet = m_DOFHandler.GetSlaveDOFSize ();

    auto K4 = EigenSparse (nNSet, nNSet);
    convertTripletsToMKLSparse (m_TriK4ff.begin (), m_TriK4ff.end (), K4);
    m_TriK4ff.clear ();

    if (NeedComputeControlMatrix ())
    {
        auto GmT = m_Gm.transpose ();
        condense (m_Gm, GmT, m_TriK4fm, m_TriK4mm, nNSet, nMSet, _OUT K4);
    }

    /**  单点约束*/
    const auto  nFSet = static_cast<int> (m_DOFHandler.ComputeSparseMatrixSize ());
    EigenSparse ksf;  // dummy
    EigenSparse kss;  // dummy
    EigenSparse kfs;
    AssemblyEigen::Matrixchunks (K4, nFSet, _OUT K4, _OUT kfs, _OUT ksf, _OUT kss);

    return K4;
}

/**
 *  Real 类型的位移恢复
 */
template <>
Pre_DECLSPEC void AssemblyEigen::RecoveryDisplacement (EigenVector& displacement,
                                                       _OUT std::vector<EigenVector::value_type>& extendDisplacement) const
{
    std::size_t       nDisp = displacement.size ();
    const std::size_t nSPC  = m_Ys.size ();
    SGInfo << "spc disp \n";
    displacement.conservativeResize (nDisp + nSPC);
    for (std::size_t i = 0; i < nSPC; ++i)
    {
        displacement (nDisp + i) = m_Ys.coeffRef (i);
    }

    SGInfo << "mpc disp \n";
    // 恢结构整体的复位移向量 displacement（包括约束自由度）
    if (m_Gm.cols () != 0)
    {
        const auto um = m_Gm * displacement;
        const auto originLength = displacement.rows ();
        const auto nGlobalDofM  = static_cast<std::size_t> (um.rows ());

        displacement.conservativeResize (GetTotalDOFSize ());
        if (nGlobalDofM != 0)
        {
            displacement.segment (originLength, nGlobalDofM) = um;
        }
    }

    SGInfo << "convert to vector \n";
    auto nTotalDisp = GetTotalDOFSize ();
    extendDisplacement.assign (displacement.data (), displacement.data () + nTotalDisp);
    SGInfo << "Trans fini \n";
}

/**
 * std::complex<double> 类型的位移恢复
 */
template <>
Pre_DECLSPEC void AssemblyEigen::RecoveryDisplacement (Eigen::VectorXcd& displacement,
                                                       _OUT std::vector<Eigen::VectorXcd::value_type>& extendDisplacement) const
{
    std::size_t       nDisp = displacement.size ();
    const std::size_t nSPC  = m_Ys.size ();

    displacement.conservativeResize (nDisp + nSPC);
    for (std::size_t i = 0; i < nSPC; ++i)
    {
        displacement (nDisp + i) = m_Ys.coeffRef (i);
    }

    // 恢结构整体的复位移向量 displacement（包括约束自由度）
    if (m_Gm.cols () != 0)
    {
        const auto um = m_Gm * displacement;
        const auto originLength = displacement.rows ();
        const auto nGlobalDofM  = static_cast<std::size_t> (um.rows ());

        displacement.conservativeResize (GetTotalDOFSize ());
        if (nGlobalDofM != 0)
        {
            displacement.segment (originLength, nGlobalDofM) = um;
        }
    }

    auto nTotalDisp = GetTotalDOFSize ();
    extendDisplacement.assign (displacement.data (), displacement.data () + nTotalDisp);
}
#endif
